Cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling

ABSTRACT

The present invention discloses a cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling, including: S1, constructing a high-dimensional GNSS/INS deep coupling filter model; S2, generating an initialization cubature point for the constructed filter model by using standard cubature rules; S3, performing CKF filtering by using novel cubature point update rules. The present invention is suitable for high-dimensional GNSS/INS deep coupling filtering with high precision and high stability.

CROSS REFERENCE TO RELATED APPLICATIONS

This application is the national phase entry of InternationalApplication No. PCT/CN2017/080383, filed on Apr. 13, 2017, which isbased upon and claims priority to Chinese Patent Application No.201610575270.4, filed on Jul. 19, 2016, the entire contents of which areincorporated herein by reference.

TECHNICAL FIELD

The present invention relates to filtering techniques, and particularlyto a cubature Kalman filtering method suitable for high-dimensionalGNSS/INS deep coupling.

BACKGROUND

Nonlinear filtering is a common information fusion technique in thefields of integrated navigation and target tracking. Different from thesignificant nonlinearity in the target tracking model, the nonlinearerror in the integrated navigation is generally weak. However, due tofactors such as filter structure, sensor sampling rate, measurementquality and filter update rate, etc., the nonlinearity in the integratednavigation shows stronger time-varying characteristics. Especially whenthe system has a large initial error and large measurement noise, andthe system carrier operates with high maneuver, the nonlinear stateestimation based on Kalman filter (KF) is often difficult to workstably. Extended Kalman filter (EKF) is widely used in engineering. Itsolves the nonlinear problem of the system based on the Jacobian matrix.The state estimation accuracy can reach the first-order level of Taylorseries expansion, and good results are obtained when the carrier isstationary or low dynamic, but the estimation accuracy of strongnonlinear system is poor. The sampling point approximation filteringmethod represented by unscented Kalman filter (UKF) is rapidly appliedas soon as it is proposed. It replaces the Jacobian matrix of asingle-point in EKF with the expected value of the Jacobian matrix aftermultipoint sampling, and has a better smoothing effect on theuncertainty caused by nonlinearity. Cubature Kalman filter (CKF) hasbetter accuracy and stability than UKF in the state estimation ofhigh-dimensional systems. Although its mean accuracy can reach thethird-order of Taylor series in the nonlinear Gaussian noise model, theestimation accuracy of its variance is only at the first-order of theTaylor series. The UKF adopts the improved UT sampling method to dealwith the non-local sampling problem of sampling points inhigh-dimensional filtering, which improves the accuracy of varianceestimation. Because CKF adopts a symmetric sampling strategy, it cannotuse the center point weight to capture the high-order information of thevariance.

The accuracy problem and consistency problem of state error varianceestimation are more prominent in high-dimensional integrated navigationsystems. For example, in engineering, the GNSS/INS loose integrationgenerally adopts 15 dimensions, and the tight integration generallyadopts 17 or higher dimensions. However, in deep coupling, due to theneed to ensure the auxiliary effect of the INS output on the GNSStracking loop, more inertial device state errors are required.Meanwhile, in order to eliminate the correlation error between theauxiliary information and the measured value, the state error variableof the code tracking loop and carrier tracking loop are required to beincreased into the state variable of the integrated filter. Taking theconventional four visible satellites as an example, the state variableof the increased tracking loop is 12 dimensions, so the existingnonlinear filtering method is difficult to apply to the state estimationproblem of high-dimensional GNSS/INS deep coupling.

SUMMARY

Objective of the present invention: the present invention provides acubature Kalman filtering method suitable for high-dimensional GNSS/INSdeep coupling in view of the problems existing in the prior art.

Technical solution: the cubature Kalman filtering method suitable forhigh-dimensional GNSS/INS deep coupling as described in the presentinvention includes:

S1, constructing a high-dimensional GNSS/INS deep coupling filter model;

S2, generating an initialization cubature point for the constructedfilter model by using standard cubature rules;

S3, performing CKF filtering by using novel cubature point update rules.

Further, the step S1 includes:

S11, setting the INS state variable of the subsystem of the INS as x_(I)and the GNSS state variable of the subsystem of the GNSS as x_(G),respectively, wherein,x _(I) =[δP δV ψ k _(a) b _(a) k _(ω) b ₁₀₇ ],x _(G) =[b _(c) d _(c) δp_(dll) δϕ_(pll) δf_(pll)],and wherein in the formula, [δP δV ψ k_(a) b_(a) k_(ω) b₁₀₇ ] arerespectively and successively a 3D position error, a velocity error, anattitude error, an accelerometer coefficient error, an accelerometercoefficient null bias, a gyroscope coefficient error and a constantvalue drift; [b_(c) d_(c) δp_(dll) δϕ_(pll) δf_(pll) ] are respectivelyand successively a receiver clock bias, a clock drift, aphase-discrimination pseudo-range error of the code tracking loop, aphase error of the carrier tracking loop, and a frequency error of thecarrier tracking loop;

S12, establishing a system model of GNSS/INS deep coupling according tothe state variables x_(I) and x_(G) as:

${\begin{bmatrix}{\overset{.}{x}}_{I} \\{\overset{.}{x}}_{G}\end{bmatrix} = {{\begin{bmatrix}{F_{I}(t)} & O_{21 \times 2} & O_{21 \times 4} & O_{21 \times 8} \\O_{2 \times 21} & {F_{G}(t)} & O_{2 \times 4} & O_{2 \times 8} \\{F_{ID}(t)} & O_{4 \times 2} & {F_{D}(t)} & O_{4 \times 8} \\{F_{IP}(t)} & O_{8 \times 2} & O_{8 \times 4} & {F_{p}(t)}\end{bmatrix}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + {G\begin{bmatrix}W_{I} \\W_{G}\end{bmatrix}}}},$

wherein in the formula,

${{F_{I}(t)} = \begin{bmatrix}{F_{N}(t)} & {F_{S}(t)} \\O_{9 \times 9} & {F_{M}(t)}\end{bmatrix}_{21 \times 21}},$

F_(N) (t) is a 9-dimensional inertial navigation basic system matrix andO_(3×9),

${{F_{S}(t)} = \begin{bmatrix}C_{a} & O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & {C_{b}^{n}(t)} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & C_{\omega} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} & {C_{b}^{n}(t)}\end{bmatrix}_{12 \times 12}},$

F_(M)(t)=[O_(12×12)], O_(i×j) is a null matrix with i rows and jcolumns, C_(a) and C_(ω) are an accelerometer coefficient error matrixand a gyroscope coefficient error matrix, respectively, C_(b) ^(n) (t)is an attitude matrix at t moment,

${{F_{G}(t)} = \begin{bmatrix}0 & 1 \\0 & {- \frac{1}{T_{r}}}\end{bmatrix}},{{F_{P}(t)} = \begin{bmatrix}O_{4 \times 4} & {2{\pi \cdot I_{4 \times 4}}} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}} & {2\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}}\end{bmatrix}_{8 \times 8}},$

T₁ and T₂ are parameters of the loop filter,

${{F_{ID}(t)} = \begin{bmatrix}{{- {\overset{\rightarrow}{L}}_{1}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{1}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{2}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{2}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{3}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{3}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{4}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{4}^{T} \cdot C_{n}^{e}} & O_{1 \times 15}\end{bmatrix}_{4 \times 21}},{{F_{D}(t)} = {- \begin{bmatrix}K_{dll} & 0 & 0 & 0 \\0 & K_{dll} & 0 & 0 \\0 & 0 & K_{dll} & 0 \\0 & 0 & 0 & K_{dll}\end{bmatrix}_{4 \times 4}}},{{F_{IP}(t)} = \begin{bmatrix}{O_{4 \times 4}\mspace{14mu} 2{\pi \cdot I_{4 \times 4}}\mspace{14mu} O_{4 \times 13}} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}\mspace{14mu} 2\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}\mspace{14mu} O_{4 \times 13}}\end{bmatrix}_{8 \times 21}},$

{dot over (x)}₁ and {dot over (x)}_(G) are differential terms of x_(I)and x_(G), respectively, G is a system-noise-driven matrix, W_(I) andW_(G) are system noise of INS, GNSS and tracking loops, T_(r) is arelevant time, K_(pll) is a carrier loop gain, I_(i×j) is a unit matrixwith i rows and j columns, {right arrow over (L)}^(T)=[{right arrow over(L)}₁ ^(T) {right arrow over (L)}₂ ^(T) {right arrow over (L)}₃ ^(T){right arrow over (L)}₄ ^(T)] is a radial position vector matrix ofsatellite and receiver, W is a radial velocity error of satellite andreceiver caused by INS position calculation errors, C_(n) ^(e) is aconversion matrix of navigation coordinate system to earth-centered,earth-fixed coordinate system, K_(dll) is a code loop gain;

S13, establishing a measurement model of GNSS/INS deep couplingaccording to the system model of GNSS/INS deep coupling as:θp=p _(I) −p _(G) =θp _(I)−(b _(c) +v _(p) +θp _(dll)),θ{dot over (p)}={dot over (p)} _(I) −{dot over (p)} _(G) =θ{dot over(p)} _(I)−(d _(c) +v _({dot over (p)}) +θ{dot over (p)} _(pll)),

wherein in the formula, θp and θ{dot over (p)} are observed quantitiesof the pseudo-range and pseudo-range rate of the satellite channelrespectively, p_(I) and {dot over (p)}_(I) are the pseudo-range and thepseudo-range rate predicted by INS respectively, p_(G) and {dot over(p)}_(G) are the pseudo-range and the pseudo-range rate measured by GNSSrespectively, b_(c) and d_(c) the clock bias and clock drift of thereceiver respectively, v_(p) and v_({dot over (p)})are observation noiseof the pseudo-range and pseudo-range rate respectively, θp_(dll) andθ{dot over (p)}_(pll) are the pseudo-range errors and pseudo-range rateerrors of the tracking loop respectively, v is a measurement noisematrix, that is

${\begin{bmatrix}{\delta\rho} \\{\delta\overset{.}{\rho}}\end{bmatrix} = {{{H(t)}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + v}},{{H(t)} = \begin{bmatrix}{{{\overset{\rightarrow}{L}}^{T}C_{n}^{e}\mspace{14mu} O_{4 \times 18}}\mspace{14mu} - {I_{4 \times 1}\mspace{14mu} O_{4 \times 1}}\mspace{14mu} - {I_{4 \times 4}\mspace{14mu} O_{4 \times 4}\mspace{14mu} O_{4 \times 4}}} \\{{O_{4 \times 3}\mspace{14mu} O_{4 \times 18}\mspace{14mu} O_{4 \times 1}}\mspace{14mu} - {I_{4 \times 1}\mspace{14mu} O_{4 \times 4}\mspace{14mu} O_{4 \times 4}\mspace{14mu} c\text{/}{f_{L\; 1} \cdot I_{4 \times 4}}}}\end{bmatrix}_{8 \times 35}},$

f_(L1) is a GPS L1 carrier frequency, and c is speed of light.

Further, the step S2 includes:

S21, calculating a state posterior distribution at {circumflex over(x)}_(k−1) at k−1 moment by using a high-precision nonlinear filteringmethod;

wherein, {circumflex over (x)}_(k−1)˜N({circumflex over(x)}_(k−1|k−1),P_(k−1|k−1)), {circumflex over (x)}_(k−1) represents thestate posterior distribution at k−1 moment, {circumflex over(x)}_(k−1|k−1) represents the state distribution at k−1 momentspeculated from the measurement at k−1 moment, P_(k−|k−1) represents acovariance of the {circumflex over (x)}_(k−1|k−1), x˜N(μ,σ²) representsthat x obeys a Gaussian distribution with a mean of μ and a variance ofσ²;

S22, generating cubature points required during the prediction processof the system state equation f(x) by using the standard cubature rules;

wherein,

$\left\{ {\begin{matrix}{X_{i,{{k - 1}❘{k - 1}}}^{+} = {{S_{{k - 1}❘{k - 1}}\xi_{i}} + {\hat{x}}_{{k - 1}❘{k - 1}}}} \\{\xi_{i} = {\sqrt{n_{x}}\left\lbrack {I_{n_{x}} - I_{n_{x}}} \right\rbrack}_{i}}\end{matrix},} \right.$

wherein n the formula, x is a state variable of the GNS SANS deepcoupling filtering model, and its dimension is n_(x), I_(n) _(x) is an_(x) dimensional unit square matrix, X_(i,k−1|k−1) represents thei^(th) cubature point at k−1 moment speculated from the measurement atk−1 moment, ξ₁ is the i^(th) standard cubature point in CKF filtering,i=1, . . . , 2n_(x), S_(k−1|k−1) satisfies theP_(k−1|k−1)=S_(k−1|k−1)(S_(k−1|k−1))^(T).

Wherein, the high-precision nonlinear filtering method includes: settingthe measurement equation of an iterated nonlinear filtering as:

x _(k|k) ^(j+1) =x _(k|k) ^(j)+α_(j) {K _(j)(z _(k) −h(x _(k|k) ^(j))−(p_(k|k−1) ⁻¹ p _(xz,k|k−1))^(T)ϑ_(j))+ϑ_(j)},

wherein x_(k|k) ^(j) is a state estimate value of the j^(th) measurementiteration at k moment, z_(k) is a measured value output by a phasediscriminator of the multi-satellite channel at k moment, h(·) is ameasurement equation of the deep coupling tracking loop, ϑ_(j)=x_(k|k)^(j)−{circumflex over (x)}_(k|k−1) is an error between a posteriorestimate and a priori estimate in the current iteration, K_(j) is aKalman gain of the j^(th) iteration, α_(j) is an iterated step-sizecontrol coefficient set by accelerating convergence, satisfying 0<α_(j)≤p_(k|k−1) ⁻¹ is an inverse of P_(k|k−1), P_(xz,k|k−1) is ancross-covariance generated during a prediction process, a covariancematrix of state posterior estimation errors takes a result of the lastiteration calculation.

Further, step S3 includes:

S31, calculating the state priori distribution at k moment by using thefollowing formula:

$\left\{ {\begin{matrix}{x_{k}^{-} \sim {N\left( {{\hat{x}}_{k❘{k - 1}},P_{k❘{k - 1}}} \right)}} \\{{\hat{x}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)}}}} \\{P_{k❘{k - 1}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)^{T}}} + Q_{k - 1}}}\end{matrix},} \right.$

wherein in the formula, x_(k) ⁻represents a state priori estimate at kmoment, {circumflex over (x)}_(k|k−1) is its mean, P_(k|k−1) is itsvariance, {circumflex over (x)}_(k|k−1) represents the state estimate atk moment speculated from the measurement and state at k−1 moment,P_(k|k−1) represents the covariance of {circumflex over (x)}_(k|k−1),w_(i)=1/2n_(x), and Q_(k−1) is the system noise variance matrix;

S32, calculating the cubature point error matrix {tilde over(X)}_(i,k|k−1) ⁻ of the prediction process using the following formula,and defining Ξ_(k)=P_(k|k−1) −Q/_(k−1) as an error variance of the Sigmapoint statistical linear regression (SLR) in a priori PDF approximationprocess;{tilde over (X)} _(i,k|k−1) ⁻ =X _(i,k|k−1) −{tilde over (x)} _(k|k−1),0≤i ≤2n _(x),

-   -   wherein X_(i,k|k−1)=f(X_(i,k 1k 1) ⁺) is the cubature point        after X_(i,k−1|k−1) ⁺ propagates through the system equation;

S33, taking the cubature point after the propagation of the systemequation as a cubature point of the measurement update process;

S34, using CKF measurement update to calculate a likelihood distributionfunction of the measured value;

wherein,

$\left\{ {\begin{matrix}{z_{k}^{-} \sim {N\left( {{\hat{z}}_{k❘{k - 1}},P_{{zz},{k❘{k - 1}}}} \right)}} \\{{\hat{z}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{h\left( X_{i,{k❘{k - 1}}} \right)}}}} \\{P_{{zz},{k❘{k - 1}}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}}} + R_{k}}}\end{matrix},} \right.$

wherein in the formula, z_(k) ⁻ represents a measurement likelihoodestimate at k moment, {tilde over (z)}_(kk−1) is its mean, P_(zz,k|k−1)is its variance, {tilde over (z)}_(k|k 1) is a measurement at k momentpredicted from the state at k−1 moment, h(x) is the measurementequation, w_(i)=1/2n_(x), and R_(k) is the measurement noise variancematrix;

S35, calculating a posterior distribution function of a state quantityx;

wherein,

$\left\{ {\begin{matrix}{x_{k}^{+} \sim {N\left( {{\hat{x}}_{k❘k},P_{k❘k}} \right)}} \\{{\hat{x}}_{k❘k} = {{\hat{x}}_{k❘{k - 1}} + {K_{k}\left( {z_{k} - {\hat{z}}_{k❘{k - 1}}} \right)}}} \\{P_{k❘k} = {P_{k❘{k - 1}} - {K_{k}P_{{zz},{k❘{k - 1}}}K_{k}^{T}}}} \\{P_{{xz},{k❘{k - 1}}} = {\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{f\left( x_{i,{{k - 1}❘{k - 1}}} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{h\left( x_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}}}}\end{matrix},} \right.$

wherein in the formula, x_(k) ¹ is a posterior estimate of the statevariable at k moment, its mean and variance are {tilde over (x)}_(k|k)and P_(k|k), respectively, K_(k)=P_(xz,k|k−1)(P_(zz,k|k−1))⁻¹ is aKalman gain matrix, P_(xz,k|k−1) is a cross-covariance between aposterior estimate of the state variable and the measurement likelihoodestimate;

S36, defining an error caused by the Sigma point approximation to theposterior distribution as Ξ_(k) ⁺=P_(k|k,)w=[w₁ w₂ ] is a weight of aCKF cubature point SLR, the SLR of the priori distribution at k momentshould accurately capture the mean and covariance of the state, andconsider the effects of system uncertainty and noise, then{tilde over (X)} _(i,k|k−1) ⁻ W=0,{tilde over (X)} _(i,k|k−1) ^(−diag() w)X _(i,k|k−1) ⁻ ^(T) =P _(k|k−1)−Q _(k),

wherein in the formula, {tilde over (X)}_(i,k|k−1) is a cubature pointerror matrix of the prediction process, Σ=diag(W) represents that thematrix Σ is constructed using W diagonal elements, in an SLR of asimilar posterior distribution, the cubature point can at leastaccurately match the mean and variance of the state, namely,{tilde over (X)} _(i,k|k) ⁺ w=0,{tilde over (X)} _(i,k|k) ⁺diag(W)({tilde over (X)}i,k|k ⁺)^(Ti) =P_(k|k),

wherein in the formula, {tilde over (X)}_(i,k|k) ⁺ is an updatedcubature point;

S37, assuming both Ξ_(k) ⁻ and Ξ_(k) ⁻ are symmetric positive definitematrices, and {tilde over (X)}_(i,k|k) ⁺=B·{tilde over (X)}_(i,k|k−1) ⁻,then Ξ_(k) ⁻=L_(k)(L_(k))^(T), Ξ_(k) ⁺=L_(k+1)(L_(k+1))^(T), wherein Bis a transformation matrix to be solved, {tilde over (X)}_(i,k|k) ⁺is anupdated cubature point error matrix; further Ξ_(k)⁺=BL_(k)(L_(k))^(t)B^(T), B=L_(k+1)Γ(L_(k))⁻¹, wherein Γ is an arbitraryorthogonal matrix that satisfies ΓT^(T)=I_(n) _(x) , when Γ is taken asunit matrix, B=L_(k+1)(L_(k))⁻¹ is obtained;

S38, obtaining an updated cubature point based on the posterior stateestimate mean {circumflex over (x)}_(k|k) and updated cubature pointerror matrix as X_(i,k|k) ⁺={circumflex over (x)}_(k|k)+{tilde over(X)}_(i,k|k) ⁺, 0 ≤i≤2n_(x).

Further, an equation of the GNSS code tracking phase-discriminationpseudo-range error ρ_(dll) isδ{dot over (ρ)}_(dll) =−K _(dll)δρ_(dll) +δV _(aid) +K _(dll) Q,

wherein θρ_(dll) is an output of the code loop filter {dot over(ρ)}_(dll) is a differential of the p_(dll), K_(dll) is a code loopgain, θV_(aid) is a speed assistance provided by the INS to the codetracking loop after filtering output calibration, Q is the system noisecaused by loop thermal noise and interference.

Further, equations of the carrier loop errors are

${{\delta\;{\overset{.}{\phi}}_{pll}} = {2\;{\pi\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)}}},{{\delta\;{\overset{.}{f}}_{pll}} = {\left\lbrack {{2\;\pi\frac{T_{2}}{T_{1}}\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)} + \frac{\delta\;\phi_{pll}}{T_{1}}} \right\rbrack \cdot K_{pll}}},$

wherein, ϕ_(dll) is the phase error of the carrier loop, f_(pll) is thefrequency error of the carrier loop, K_(pll) is a loop gain, andθf_(aid) is a Doppler frequency error assistance provided by the INS tothe carrier tracking loop after filtering output calibration, T₁ and T₂are parameters of the loop

filter, that is, the transfer function of the loop low-pass filter is

${F(s)} = {\frac{{T_{2}s} + 1}{T_{1}s}.}$

Advantageous effects: compared with the prior art, the present inventionhas the following significant advantages:

(1) initializing and generating the cubature point by using a higherprecision nonlinear filtering method;

(2) updating the cubature point based on matrix transformation, whichimproves the consistency of the prediction process variance in the stateestimation process, and improves the stability of the filter;

(3) using the state error parameter of the tracking loop as the stateparameter of the integrated filter to eliminate the correlation betweenthe filter measurement and the tracking loop errors, which improves theaccuracy and stability of the INS auxiliary quantity calibration;

(4) using two kinds of strategies to generate the cubature point of CKF,that is, the cubature point is generated by using a time-consuming andhigh-precision method during initialization, and the subsequent cubaturepoint update process is realized based on the variance of the stateestimation error at the previous moment, which improves the utilizationof the cubature point.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a system block diagram according to one embodiment of thepresent invention.

FIG. 2 is a schematic diagram of high-dimensional GNSS/INS deep couplingfiltering model.

DETAILED DESCRIPTION OF THE EMBODIMENTS

As shown in FIG. 1, the cubature Kalman filtering method suitable forhigh-dimensional GNSS/INS deep coupling of the present embodimentincludes the following steps:

S1, constructing a high-dimensional GNSS/INS deep coupling filter model,as shown in FIG. 2.

Step S1 specifically includes:

S11, setting the INS state variable of the subsystem of the INS as x_(I)and the GNSS state variable of the subsystem of the GNSS as x_(G),respectively, wherein,x_(I)=[θP θV ψ k_(α)b_(α) k_(ω) b_(ω],)x_(G)=[b_(c) d_(c) δρ_(dll) δϕ_(pll) δf_(pll)],

and wherein in the formula, [δP δV ψ k_(a) b_(a) k_(ω) b₁₀₇ ] arerespectively and successively a 3D position error, a velocity error, anattitude error, an accelerometer coefficient error, an accelerometercoefficient null bias, a gyroscope coefficient error and a constantvalue drift; [b_(c) d_(c) δρ_(dll) δϕ_(pll) δf_(pll)] are respectivelyand successively a receiver clock bias, a clock drift, aphase-discrimination pseudo-range error of the code tracking loop, aphase error of the carrier tracking loop, and a frequency error of thecarrier tracking loop.

Wherein an equation of the GNSS code tracking phase-discriminationpseudo-range error ρ_(dll) isδ{dot over (ρ)}_(dll) =−K _(dll)δρ_(dll) +δV _(aid) +K _(dll) Q,

wherein δρ_(dll) is an output of the code loop filter, {dot over(ρ)}_(dll) is a differential of the ρ_(dll) K_(dll) is a code loop gain,δV_(aid) is a speed assistance provided by the INS to the code trackingloop after filtering output calibration, Q is the system noise caused byloop thermal noise and interference.

Wherein equations of the carrier loop errors are

${{\delta\;{\overset{.}{\phi}}_{pll}} = {2\;{\pi\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)}}},{{\delta\;{\overset{.}{f}}_{pll}} = {\left\lbrack {{2\;\pi\frac{T_{2}}{T_{1}}\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)} + \frac{\delta\;\phi_{pll}}{T_{1}}} \right\rbrack \cdot K_{pll}}},$

wherein, ϕ_(pll) is the phase error of the carrier loop, f_(pll) is thefrequency error of the carrier loop, K_(dll) is a loop gain, andδf_(aid) is a Doppler frequency error assistance provided by the INS tothe carrier tracking loop after filtering output calibration, T₁ and T2are parameters of the loop filter, that is, the transfer function of theloop low-pass filter is

${F(s)} = {\frac{{T_{2}s} + 1}{T_{1}s}.}$

S12, establishing a system model of GNSS/INS deep coupling according tothe state variables x_(I), and x_(G) as:

${\begin{bmatrix}{\overset{.}{x}}_{I} \\{\overset{.}{x}}_{G}\end{bmatrix} = {{\begin{bmatrix}{F_{I}(t)} & O_{21 \times 2} & O_{21 \times 4} & O_{21 \times 8} \\O_{2 \times 21} & {F_{G}(t)} & O_{2 \times 4} & O_{2 \times 8} \\{F_{ID}(t)} & O_{4 \times 2} & {F_{D}(t)} & O_{4 \times 8} \\{F_{IP}(t)} & O_{8 \times 2} & O_{8 \times 4} & {F_{p}(t)}\end{bmatrix}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + {G\begin{bmatrix}W_{I} \\W_{G}\end{bmatrix}}}},$

wherein in the formula,

${{F_{I}(t)} = \begin{bmatrix}{F_{N}(t)} & {F_{S}(t)} \\O_{9 \times 9} & {F_{M}(t)}\end{bmatrix}_{21 \times 21}},$

F_(N) (t) is a 9-dimensional inertial navigation basic System matrix andO_(3×9),

${{F_{S}(t)} = \begin{bmatrix}C_{a} & O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & {C_{b}^{n}(t)} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & C_{\omega} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} & {C_{b}^{n}(t)}\end{bmatrix}_{12 \times 12}},$

F_(M)(t)=[O_(12×12)], O_(i×j) is a null matrix with i rows and jcolumns, C_(α) and C_(ω) are an accelerometer coefficient error matrixand a gyroscope coefficient error matrix respectively, C_(b) ^(n)(t) isan attitude matrix at t moment,

${{F_{G}(t)} = \begin{bmatrix}0 & 1 \\0 & {- \frac{1}{T_{r}}}\end{bmatrix}},{{F_{P}(t)} = \begin{bmatrix}O_{4 \times 4} & {2\;{\pi \cdot I_{4 \times 4}}} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}} & {2\;\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}}\end{bmatrix}_{8 \times 8}},$

T₁ and T₂ are parameters of the loop filter,

${{F_{ID}(t)} = \begin{bmatrix}{{- {\overset{\rightarrow}{L}}_{1}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{1}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{2}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{2}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{3}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{3}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{4}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{4}^{T} \cdot C_{n}^{e}} & O_{1 \times 15}\end{bmatrix}_{4 \times 12}},{{F_{D}(t)} = {- \begin{bmatrix}K_{dll} & 0 & 0 & 0 \\0 & K_{dll} & 0 & 0 \\0 & 0 & K_{dll} & 0 \\0 & 0 & 0 & K_{dll}\end{bmatrix}_{4 \times 4}}},{{F_{IP}(t)} = \begin{bmatrix}O_{4 \times 4} & {2\;{\pi \cdot I_{4 \times 4}}} & O_{4 \times 13} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}} & {2\;\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}} & O_{4 \times 13}\end{bmatrix}_{8 \times 21}},$

{dot over (x)}_(I) and {dot over (x)}_(G) are differential terms ofx_(I) and x_(G) respectively, G is a system-noise-driven matrix, W_(I)and W_(G) are system noise of INS, GNSS and tracking loops, T_(r) is arelevant time, K_(pll) is a carrier loop gain, I_(i×j) is a unit matrixwith i rows and j columns, {right arrow over (L)}^(T)=[{right arrow over(L)}₁ ^(T) {right arrow over (L)}₂ ^(T) {right arrow over (L)}₃ ^(T){right arrow over (L)}₄ ^(T)] is a radial position vector matrix ofsatellite and receiver, W is a radial velocity error of satellite andreceiver caused by INS position calculation errors, C_(n) ^(e) is aconversion matrix of navigation coordinate system to earth-centered,earth-fixed coordinate system, K_(dll) is a code loop gain.

S13, establishing a measurement model of GNSS/INS deep couplingaccording to the system model of GNSS/INS deep coupling as:δρ=ρ_(I)−ρ_(G)=δρ_(I)−(b _(c) +v _(ρ)+δρ_(dll)),δ{dot over (ρ)}={dot over (ρ)}_(I)−{dot over (ρ)}_(G)=δ{dot over(ρ)}_(I)−(d _(c) +v _({dot over (ρ)})+δ{dot over (ρ)}_(pll)),

wherein in the formula, δρ and δ{dot over (ρ)} are observed quantitiesof the pseudo-range and pseudo-range rate of the satellite channelrespectively, ρ_(I) and {dot over (ρ)}_(I) are the pseudo-range and thepseudo-range rate predicted by INS respectively, ρ_(G) and {dot over(ρ)}_(G) are the pseudo-range and the pseudo-range rate measured by GNSSrespectively, b_(c) and d_(c) are the clock bias and clock drift of thereceiver respectively, v_(ρ) and v_(ρ) are observation noise of thepseudo-range and pseudo-range rate respectively, δρ_(dll) and δ{dot over(ρ)}_(pll) are the pseudo-range errors and pseudo-range rate errors ofthe tracking loop respectively, v is a measurement noise matrix, that is

$\mspace{20mu}{{\begin{bmatrix}{\delta\;\rho} \\{\delta\;\overset{.}{\rho}}\end{bmatrix} = {{{H(t)}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + v}},{{H(t)} = \begin{bmatrix}{{\overset{\rightarrow}{L}}^{T}C_{n}^{e}} & O_{4 \times 18} & {- I_{4 \times 1}} & O_{4 \times 1} & {- I_{4 \times 4}} & O_{4 \times 4} & O_{4 \times 4} \\O_{4 \times 3} & O_{4 \times 18} & O_{4 \times 1} & {- I_{4 \times 1}} & O_{4 \times 4} & O_{4 \times 4} & {{c/f_{L\; 1}} \cdot I_{4 \times 4}}\end{bmatrix}_{8 \times 35}},}$

f_(L1) is a GPS L1 carrier frequency, and c is speed of light.

S2, generating an initialization cubature point for the constructedfilter model by using standard cubature rules.

Step S2 specifically includes:

S21, calculating a state posterior distribution {circumflex over(x)}_(k−1) at k−1 moment by using a high-precision nonlinear filteringmethod;

wherein, {circumflex over (x)}_(k−1)˜N({circumflex over (x)}_(k−1|k−1),P_(k−1|k−1) {circumflex over (x)}_(k−1) represents the state posteriordistribution at k−1 moment, {circumflex over (x)}_(k−1|k−1) representsthe state distribution at k−1 moment speculated from the measurement atk−1 moment, P_(k−1|k−1) represents a covariance of the {circumflex over(x)}_(k−1|k−1), x˜N(μ, σ² ) represents that x obeys a Gaussiandistribution with a mean of μ and a variance of σ².

Wherein, the high-precision nonlinear filtering method includes: settingthe measurement equation of an iterated nonlinear filtering as:x _(k|k) ^(j+1) =x _(k|k) ^(j)+α_(j) {K _(j)(z _(k) −h(x _(k|k) ^(j))−(p_(k|k−1) ⁻¹ p _(xz,k|k−1))^(T)ϑ_(j))+ϑ_(j)},

where x_(k|k) ^(j) is a state estimate value of the j^(th) measurementiteration at k moment, z_(k) is a measured value output by a phasediscriminator of the multi-satellite channel at k moment, h(·) is ameasurement equation of the deep coupling tracking loop, ϑ_(j)=x_(k|k)^(j)−{circumflex over (x)}_(k|k−1) is an error between a posteriorestimate and a priori estimate in the current iteration, K_(j) is aKalman gain of the j^(th) iteration, α_(j) is an iterated step-sizecontrol coefficient set by accelerating convergence, satisfying0<α_(j)≤1, P_(k|k−1) ⁻¹ is an inverse of P_(k|k−1), P_(xz, k|k−1) is ancross-covariance generated during the prediction process, the covariancematrix of the state posterior estimation errors takes the result of thelast iteration calculation.

S22, generating cubature points required during the prediction processof the system state equation f(x) by using the standard cubature rules;

wherein,

$\left\{ {\begin{matrix}{X_{i,{{k - 1}❘{k - 1}}}^{+} = {{S_{{k - 1}❘{k - 1}}\xi_{i}} + {\hat{x}}_{{k - 1}❘{k - 1}}}} \\{\xi_{i} = {\sqrt{n_{x}}\left\lbrack {I_{n_{x}} - I_{n_{x}}} \right\rbrack}_{i}}\end{matrix},} \right.$

wherein in the formula, x is a state variable of the GNSS/INS deepcoupling filtering model, and its dimension is n_(x), I_(n) _(x) , is an_(x) dimensional unit square matrix, X_(i,k 1|k) ⁺ represents thei^(th) cubature point at k−1 moment speculated from the measurement atk−1 moment, ξ_(i) is the i^(th) standard cubature point in CKFfiltering, i=1, . . . , 2n_(x), S_(k−1|k−1)satisfies theP_(k−1|k−1)=S_(k−1|k−1) (S_(k−1|k−1))^(T).

S3, performing CKF filtering by using novel cubature point update rules.

Step S3 includes:

S31, calculating the state priori distribution at k moment by using thefollowing formula:

$\left\{ {\begin{matrix}{{x_{k}^{-} \sim {N\left( {{\hat{x}}_{k❘{k - 1}},P_{k❘{k - 1}}} \right)}}\mspace{495mu}} \\{{{\hat{x}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)}}}}\mspace{436mu}} \\{P_{k❘{k - 1}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)^{T}}} + Q_{k - 1}}}\end{matrix},} \right.$

wherein in the formula, x_(k) ⁻represents a state priori estimate at kmoment, {circumflex over (x)}_(k|k−1) is its mean, P_(k|k−1) is itsvariance, {circumflex over (x)}_(k|k−1) represents the state estimate atk moment speculated from the measurement and state at k−1 moment,P_(k|k−1) represents the covariance of {circumflex over (x)}_(k|k−1),w_(i)=1/2n_(x), and Q_(k−1) is the system noise variance matrix;

S32, calculating the cubature point error matrix X _(i,k|k−1) ⁻of theprediction process using the following formula, and defining Ξ_(k)^(−=P) _(k|k−1) −Q_(k−1) as an error variance of the Sigma pointstatistical linear regression (SLR) in a priori PDF approximationprocess;{tilde over (X)} _(i,k|k−1) ⁻ =X _(i,k|k−1) −{tilde over (x)} _(k|k−1),0≤i≤2n _(x),

wherein X_(i,k|k−1)=f(X_(i,k 1k 1) ⁺) is the cubature point afterX_(i,k−1|k−1) ⁺ propagates through the system equation;

S33, taking the cubature point after the propagation of the systemequation as a cubature point of the measurement update process;

S34, using CKF measurement update to calculate a likelihood distributionfunction of the measured value;

wherein,

$\left\{ {\begin{matrix}{{z_{k}^{-} \sim {N\left( {{\hat{z}}_{k❘{k - 1}},P_{{zz},{k❘{k - 1}}}} \right)}}\mspace{419mu}} \\{{{\hat{z}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{h\left( X_{i,{k❘{k - 1}}} \right)}}}}\mspace{405mu}} \\{P_{{zz},{k❘{k - 1}}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}}} + R_{k}}}\end{matrix},} \right.$

wherein in the formula, z_(k) ⁻ represents a measurement likelihoodestimate at k moment, {tilde over (z)}_(k|k−1) is its mean, P_(zz,k|k−1)is its variance, {tilde over (z)}_(k|k−1) is a measurement at k momentpredicted from the state at k−1 moment, h(x) is the measurementequation, w_(i)=1/2n_(x), and R_(k) is the measurement noise variancematrix;

S35, calculating a posterior distribution function of a state quantityx;

wherein,

$\left\{ {\begin{matrix}{{x_{k}^{+} \sim {N\left( {{\hat{x}}_{k❘k},P_{k❘k}} \right)}}\mspace{439mu}} \\{{{\hat{x}}_{k❘k} = {{\hat{x}}_{k❘{k - 1}} + {K_{k}\left( {z_{k} - {\hat{z}}_{k❘{k - 1}}} \right)}}}\mspace{320mu}} \\{{P_{k❘k} = {P_{k❘{k - 1}} - {K_{k}P_{{zz},{k❘{k - 1}}}K_{k}^{T}}}}\mspace{320mu}} \\{P_{{xz},{k❘{k - 1}}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}\left( {f\left( {{X_{i,{{k - 1}❘{k - 1}}}\left( {- {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}} \right.} \right.}}}\end{matrix},} \right.$

wherein in the formula, x_(k) ⁺ is a posterior estimate of the statevariable at k moment, its mean and variance are {circumflex over(x)}_(k|k), and P_(k|k), respectively,K_(k)=P_(xz,k|k−1)(P_(zz,k|k−1))⁻¹ is a Kalman gain matrix, P_(xz,k|k−1)is a cross-covariance between a posterior estimate of the state variableand the measured likelihood estimate;

S36, defining an error caused by the Sigma point approximation to theposterior distribution as Ξ_(k) ⁺=P_(k|k), w=[w₁ . . . w_(2n) _(x) ] isa weight of a CKF cubature point SLR, the SLR of the priori distributionat k moment should accurately capture the mean and covariance of thestate, and consider the effects of system uncertainty and noise, then{tilde over (X)} _(i,k|k−1) ⁻ W=0,{tilde over (X)} _(i,k|k−1) ^(−diag() w)X _(i,k|k−1) ⁻ ^(T) =P _(k|k−1)−Q _(k),

wherein in the formula, {tilde over (X)}_(i,k|k−1) is a cubature pointerror matrix of the prediction process, Σ=diag(W) represents that thematrix Σ is constructed using W diagonal elements, in an SLR of asimilar posterior distribution, the cubature point can at leastaccurately match the mean and variance of the state, namely,{tilde over (X)} _(i,k|k) ⁺ w=0,{tilde over (X)} _(i,k|k) ⁺diag(W)({tilde over (X)}i,k|k ⁺)^(Ti) =P_(k|k),

wherein in the formula, {tilde over (X)}_(i,k|k) ⁺ is an updatedcubature point;

S37, assuming both Ξ_(k) ⁻ and Ξ_(k) ⁻ are symmetric positive definitematrices, and {tilde over (X)}_(i,k|k)=B·{tilde over (X)}_(i,k|k−1) ⁻,then Ξ_(k) ⁻=L_(k)(L_(k))^(T), μ_(k) ⁺=L_(k+1)(L_(k+1))^(T), wherein Bis a transformation matrix to be solved, {tilde over (X)}_(i,k|k) ⁺is anupdated cubature point error matrix; further Ξ_(k)⁺=BL_(k)(L_(k))^(T)B^(T), B=L_(k+1)Γ(L_(k))⁻¹, wherein Γ is an arbitraryorthogonal matrix that satisfies ΓΓ^(T)=I_(n) _(x) , when Γ is taken asunit matrix, B=L_(k+1)(L_(k))⁻¹ is obtained;

S38, obtaining an updated cubature point based on the posterior stateestimate mean {circumflex over (x)}_(k|k) and updated cubature pointerror matrix as X_(i,k|k) ⁺={circumflex over (x)}_(k|k)+{tilde over(X)}_(i,k|k) ⁺0 ≤i≤2n_(x).

The above are only preferred embodiments of the present invention, andthe scope of the present invention is not limited thereto. Therefore,equivalent changes made according to the claims of the present inventionare still within the scope of the present invention.

What is claimed is:
 1. A method of integrated navigation and targettracking for high-dimensional GNSS/INS deep coupling comprising acubature Kalman filtering (CKF) method, wherein the cubature Kalmanfiltering method comprises: S1, constructing a high-dimensional GNSS/INSdeep coupling filter model to obtain a constructed filter model; S2,generating an initialization cubature point for the constructed filtermodel by using standard cubature rules; S3, performing CKF filtering onthe constructed filter model by using novel cubature point update rules;wherein the step S3 comprises: S31, calculating a state prioridistribution at k moment by using the following formula:$\left\{ {\begin{matrix}{{x_{k}^{-} \sim {N\left( {{\hat{x}}_{k❘{k - 1}},P_{k❘{k - 1}}} \right)}}\mspace{495mu}} \\{{{\hat{x}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)}}}}\mspace{436mu}} \\{P_{k❘{k - 1}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{f\left( X_{i,{{k - 1}❘{k - 1}}}^{+} \right)} - {\hat{x}}_{k❘{k - 1}}} \right)^{T}}} + Q_{k - 1}}}\end{matrix},} \right.$ wherein in the formula, x_(k) ⁻represents astate priori estimate at k moment, {circumflex over (x)}_(k|k−1) is amean of the x_(k) ⁻, P_(k|k−1) is a variance of the x_(k) ⁻, {circumflexover (x)}_(k|k−1) represents a state estimate at k moment speculatedfrom a measurement and a state at k−1 moment, P_(k|k−1) represents acovariance of {circumflex over (x)}_(k|k−1), w_(i)=1/2n_(x), and Q_(k−1)is a system noise variance matrix; S32, calculating a cubature pointerror matrix {tilde over (x)}_(i,k|k−1) ⁻of the prediction process usingthe following formula, and defining Ξ_(k) ⁻=P_(k|k−1)−Q_(k−1) as anerror variance of a Sigma point statistical linear regression (SLR) in apriori PDF approximation process;{tilde over (x)}_(i,k|k−1) ⁻=x_(i,k|k−1)−{circumflex over (x)}_(k|k−1),0≤i≤2n_(x), wherein x_(i,k|k−1)=f(x_(i,k−1|k−1) ⁺) is a cubature pointafter x_(i,k−1|k−1) ⁺propagates through a system equation; S33, takingthe cubature point after the propagation of the system equation as acubature point of a measurement update process; S34, using a CKFmeasurement update to calculate a likelihood distribution function of ameasured value; wherein, $\left\{ {\begin{matrix}{{z_{k}^{-} \sim {N\left( {{\hat{z}}_{k❘{k - 1}},P_{{zz},{k❘{k - 1}}}} \right)}}\mspace{419mu}} \\{{{\hat{z}}_{k❘{k - 1}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}{h\left( X_{i,{k❘{k - 1}}} \right)}}}}\mspace{405mu}} \\{P_{{zz},{k❘{k - 1}}} = {{\sum\limits_{i = 1}^{2n_{x}}\;{{w_{i}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}}} + R_{k}}}\end{matrix},} \right.$ wherein in the formula, z_(k) ⁻represents ameasurement likelihood estimate at k moment, {circumflex over(z)}_(k|k−1) is a mean of the z_(k) ⁻, P_(zz,k|k−1) is a variance of thez_(k) ⁻, {circumflex over (z)}_(k|k−1) is a measurement at k momentpredicted from the state at k−1 moment, h(x) is a measurement equation,w_(i)=1/2n_(x), and R_(k) is a measurement noise variance matrix; S35,calculating a posterior distribution function of a state variable x;wherein $\left\{ {\begin{matrix}{{x_{k}^{+} \sim {N\left( {{\hat{x}}_{k❘k},P_{k❘k}} \right)}}\mspace{439mu}} \\{{{\hat{x}}_{k❘k} = {{\hat{x}}_{k❘{k - 1}} + {K_{k}\left( {z_{k} - {\hat{z}}_{k❘{k - 1}}} \right)}}}\mspace{320mu}} \\{{P_{k❘k} = {P_{k❘{k - 1}} - {K_{k}P_{{zz},{k❘{k - 1}}}K_{k}^{T}}}}\mspace{320mu}} \\{P_{{xz},{k❘{k - 1}}} = {\sum\limits_{i = 1}^{2n_{x}}\;{w_{i}\left( {f\left( {{X_{i,{{k - 1}❘{k - 1}}}\left( {- {\hat{x}}_{k❘{k - 1}}} \right)}\left( {{h\left( X_{i,{k❘{k - 1}}} \right)} - {\hat{z}}_{k❘{k - 1}}} \right)^{T}} \right.} \right.}}}\end{matrix},} \right.$ wherein in the formula, x_(k) ⁺, is a posteriorestimate of the state variable at k moment, a mean and a variance of thex_(k) ⁺ are {circumflex over (x)}_(k|k) and P_(k|k), respectively,K_(k)=P_(xz,k|k−1)(P_(zz,k|k−1))⁻¹ is a Kalman gain matrix, P_(xz,k|k−1)is a cross-covariance between a posterior estimate of the state variableand the measurement likelihood estimate; S36, defining an error causedby a Sigma point approximation to a posterior distribution as Ξ_(k)⁺=P_(k|k), w=[w₁ . . . w_(2n) _(x) ]is a weight of a CKF cubature pointSLR, a SLR of the priori distribution at k moment accurately captures amean and a covariance of the state, and consider effects of systemuncertainty and noise, then{tilde over (x)}_(i,k|k−1) ⁻w=0,{tilde over (x)}_(i,k|k−1) ⁻diag(w){tilde over (x)}_(i,k|k−1) ⁻^(T)=P_(k|k−1)−Q_(k), wherein in the formula, {tilde over (x)}_(i,k|k−1)⁻is the cubature point error matrix of the prediction process, Σ=diag(w)represents that the matrix Σ is constructed using w diagonal elements,in a SLR of a similar posterior distribution, the cubature point can atleast accurately match the mean and the variance of the state, namely,{tilde over (x)}_(i,k|k)w=0,{tilde over (x)}_(i,k|k) ⁺diag(w)({tilde over (x)}_(i,k|k)⁺)^(T)=P_(k|k), wherein in the formula, {tilde over (X)}_(i,k|k) ⁺is anupdated cubature point; S37, assuming both Ξ_(k) ⁻and Ξ_(k) ⁺aresymmetric positive definite matrices, and {tilde over (x)}_(i,k|k)=B·{tilde over (X)}_(i,k|K−1) ⁻, then Ξ_(k) ⁻=L_(k)(L_(k))^(T), Ξ_(k)⁺=L_(k+1)(L_(k+1))^(T) wherein B is a transformation matrix to besolved, {tilde over (x)}_(i,k|k) ⁺is an updated cubature point errormatrix; further Ξ_(k) ⁺=BL_(k) (L_(k))^(T)B^(T) , B=L_(k+1)Γ(L_(k))⁻¹,wherein Γ is an arbitrary orthogonal matrix that satisfies ΓΓ^(T)=I_(n)_(x) , when Γ is taken as a unit matrix, B=L_(k+1)(L_(k))⁻¹ is obtained;S38, obtaining an updated cubature point based on a posterior stateestimate mean {circumflex over (x)}_(k|k) and an updated cubature pointerror matrix as x⁺ _(i,k|k)={circumflex over (x)}_(k|k)={tilde over(x)}_(i,k|k) ⁺, 0≤i≤2n_(x).
 2. The method according to claim 1, whereinthe step S1 comprises: S11, setting an INS state variable of a subsystemof an INS as x_(I) and a GNSS state variable of a subsystem of a GNSS asx_(G), respectively, wherein,x_(I) =[δP δV ψ k_(a) b_(a) K₁₀₇ b_(ω],)x_(G) =[b_(c) d_(c) δρ_(dll) δϕ_(pll) δf_(pll)], and wherein in theformula, [δP δV ψk_(α)b_(α)k_(ω)b_(ω)] are respectively and successivelya 3D position error, a velocity error, an attitude error, anaccelerometer coefficient error, an accelerometer coefficient null bias,a gyroscope coefficient error and a constant value drift; [b_(c) d_(c)δρ_(dll) δϕ_(pll) δf_(pll) ] are respectively and successively areceiver clock bias, a clock drift, a phase-discrimination pseudo-rangeerror of a code tracking loop, a phase error of a carrier tracking loop,and a frequency error of a carrier tracking loop; S12, establishing asystem model of the high-dimensional GNSS/INS deep coupling according tothe INS state variable x_(I) and the GNSS state variable x_(G) as:${\begin{bmatrix}{\overset{.}{x}}_{I} \\{\overset{.}{x}}_{G}\end{bmatrix} = {{\begin{bmatrix}{F_{I}(t)} & O_{21 \times 2} & O_{21 \times 4} & O_{21 \times 8} \\O_{2 \times 21} & {F_{G}(t)} & O_{2 \times 4} & O_{2 \times 8} \\{F_{ID}(t)} & O_{4 \times 2} & {F_{D}(t)} & O_{4 \times 8} \\{F_{IP}(t)} & O_{8 \times 2} & O_{8 \times 4} & {F_{P}(t)}\end{bmatrix}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + {G\begin{bmatrix}W_{I} \\W_{G}\end{bmatrix}}}},$ wherein in the formula,${{F_{I}(t)} = \begin{bmatrix}{F_{N}(t)} & {F_{S}(t)} \\O_{9 \times 9} & {F_{M}(t)}\end{bmatrix}_{21 \times 21}},$ F_(N) (t) is a 9-dimensional inertialnavigation basic system matrix and O_(3×9),${{F_{S}(t)} = \begin{bmatrix}C_{a} & O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & {C_{b}^{n}(t)} & O_{3 \times 3} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & C_{\omega} & O_{3 \times 3} \\O_{3 \times 3} & O_{3 \times 3} & O_{3 \times 3} & {C_{b}^{n}(t)}\end{bmatrix}_{12 \times 12}},$ F_(M)(t)=[O_(12×12)], O_(i×j) is a nullmatrix with i rows and j columns, C_(α)and C_(ω)are an accelerometercoefficient error matrix and a gyroscope coefficient error matrixrespectively, C_(b) ^(n)(t) is an attitude matrix at t moment,${{F_{G}(t)} = \begin{bmatrix}0 & 1 \\0 & {- \frac{1}{T_{r}}}\end{bmatrix}},{{F_{P}(t)} = \begin{bmatrix}0_{4 \times 4} & {2{\pi \cdot I_{4 \times 4}}} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}} & {2\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}}\end{bmatrix}_{8 \times 8}},$ T₁ and T₂ are parameters of a loop filter,${{F_{ID}(t)} = \begin{bmatrix}{{- {\overset{\rightarrow}{L}}_{1}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{1}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{2}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{2}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{3}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{3}^{T} \cdot C_{n}^{e}} & O_{1 \times 15} \\{{- {\overset{\rightarrow}{L}}_{4}^{T}} \cdot C_{n}^{e} \cdot W} & {{\overset{\rightarrow}{L}}_{4}^{T} \cdot C_{n}^{e}} & O_{1 \times 15}\end{bmatrix}_{4 \times 21}},{{F_{D}(t)} = {- \begin{bmatrix}K_{dll} & 0 & 0 & 0 \\0 & K_{dll} & 0 & 0 \\0 & 0 & K_{dll} & 0 \\0 & 0 & 0 & K_{dll}\end{bmatrix}_{4 \times 4}}},{{F_{IP}(t)} = \begin{bmatrix}{O_{4 \times 4}\mspace{14mu} 2{\pi \cdot I_{4 \times 4}}\mspace{14mu} O_{4 \times 13}} \\{K_{pll}\text{/}{T_{1} \cdot I_{4 \times 4}}\mspace{14mu} 2\pi\; K_{pll}T_{2}\text{/}{T_{1} \cdot I_{4 \times 4}}\mspace{14mu} O_{4 \times 13}}\end{bmatrix}_{8 \times 21}},$ {dot over (x)}_(I) and {dot over (x)}_(G)are differential terms of x_(I) and x_(G) respectively, G is asystem-noise-driven matrix, W_(I) and W_(G) are system noise of the INS,the GNSS and tracking loops, T_(r) is a relevant time, K_(pll) is acarrier loop gain, I_(i×j) is a unit matrix with i rows and j columns,{right arrow over (L)}^(T)=[{right arrow over (L)}₁ ^(T){right arrowover (L)}₂ ^(T){right arrow over (L)}₃ ^(T){right arrow over (L)}₄ ^(T)]is a radial position vector matrix of a satellite and a receiver, W is aradial velocity error of the satellite and the receiver caused by INSposition calculation errors, C_(n) ^(e) is a conversion matrix ofnavigation coordinate system to earth-centered earth-fixed coordinatesystem, K_(dll) is a code loop gain; S13, establishing a measurementmodel of GNSS/INS deep coupling according to the system model ofGNSS/INS deep coupling as:δρ=ρ_(I)−ρ_(G)=δρ_(I)−(b_(c)+v_(ρ)+δρ_(dll)),δ{dot over (ρ)}=ρ_(I)−{dot over (ρ)}_(G)=δ{dot over(ρ)}_(I)−(b_(c)+v_({dot over (ρ)})+δρ_(dll)), wherein in the formula, δρand δ{dot over (ρ)} are observed quantities of a pseudo-range and apseudo-range rate of a satellite channel respectively, ρ_(I) and {dotover (ρ)}_(I), are a pseudo-range and a pseudo-range rate predicted bythe INS respectively, ρ_(G) and {dot over (ρ)}_(G) are a pseudo-rangeand a pseudo-range rate measured by the GNSS respectively, b_(c) andd_(c) are a clock bias and a clock drift of the receiver respectively,v_(ρ)and v_({dot over (ρ)})are observation noise of the pseudo-range andthe pseudo-range rate respectively, δρ_(dll) and δ{dot over (ρ)}_(pll)are pseudo-range errors and pseudo-range rate errors of the trackingloops respectively, v is a measurement noise matrix, that is${\begin{bmatrix}{\delta\rho} \\{\delta\overset{.}{\rho}}\end{bmatrix} = {{{H(t)}\begin{bmatrix}x_{I} \\x_{G}\end{bmatrix}} + v}},{{H(t)} = \begin{bmatrix}{{{\overset{\rightarrow}{L}}^{T}C_{n}^{e}\mspace{14mu} O_{4 \times 18}}\mspace{14mu} - {I_{4 \times 1}\mspace{14mu} O_{4 \times 1}}\mspace{14mu} - {I_{4 \times 4}\mspace{14mu} O_{4 \times 4}\mspace{14mu} O_{4 \times 4}}} \\{{O_{4 \times 3}\mspace{14mu} O_{4 \times 18}\mspace{14mu} O_{4 \times 1}}\mspace{14mu} - {I_{4 \times 1}\mspace{14mu} O_{4 \times 4}\mspace{14mu} O_{4 \times 4}\mspace{14mu} c\text{/}{f_{L\; 1} \cdot I_{4 \times 4}}}}\end{bmatrix}_{8 \times 35}},$ f_(L1) is a GPS L1 carrier frequency, andc is speed of light.
 3. The method according to claim 2, wherein anequation of the phase-discrimination pseudo-range error ρ_(dll) of thecode tracking loop isδ{dot over (ρ)}_(dll)=−K_(dll)δρ_(dll)+δV_(aid) +K_(dll)Q, whereinδρ_(dll) is an output of a code loop filter, {dot over (ρ)}_(dll) is adifferential of the ρ_(dll), K_(dll) is a code loop gain, δV_(aid) is aspeed assistance provided by the INS to the code tracking loop after afiltering output calibration, Q is system noise caused by loop thermalnoise and interferences.
 4. The method dimensional GNS SANS deepcoupling according to claim 2, wherein equations of errors of thecarrier tracking loop are${{\delta{\overset{.}{\phi}}_{pll}} = {2{\pi\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)}}},{{\delta\;{\overset{.}{f}}_{pll}} = {\left\lbrack {{2\pi\frac{T_{2}}{T_{1}}\left( {{\delta\; f_{pll}} + {\delta\; f_{aid}}} \right)} + \frac{{\delta\phi}_{pll}}{T_{1}}} \right\rbrack \cdot K_{pll}}},$wherein, ϕ_(pll) is the phase error of the carrier tracking loop,f_(pll) is the frequency error of the carrier tracking loop, andδf_(aid) is a Doppler frequency error assistance provided by the INS tothe carrier tracking loop after a filtering output calibration, T₁ andT₂ are parameters of the loop filter, that is, a transfer function ofthe loop filter is ${F(s)} = {\frac{{T_{2}s} + 1}{T_{1}s}.}$
 5. Themethod according to claim 1, wherein the step S2 comprises: S21,calculating a state posterior distribution {circumflex over (x)}_(k−1)at k−l moment by using a high-precision nonlinear filtering method;wherein, {circumflex over (x)}_(k−1) ˜N({circumflex over (x)}_(k−1|k−1),P_(k−1|k−1)), {circumflex over (x)}_(k−1) represents the state posteriordistribution at k−1 moment, {circumflex over (x)}_(k−1|k−1) representsthe state distribution at k−1 moment speculated from the measurement atk-1 moment, P_(k−1|k−1) represents a covariance of the {circumflex over(x)}_(k−1|K−1), x˜N(μ,σ²) represents that x obeys a Gaussiandistribution with a mean of μ and a variance of σ²; S22, generatingcubature points required during a prediction process of a system stateequation f(x) by using the standard cubature rules; wherein.$\left\{ {\begin{matrix}{X_{i,{{k - 1}❘{k - 1}}}^{+} = {{S_{{k - 1}❘{k - 1}}\xi_{i}} + {\hat{x}}_{{k - 1}❘{k - 1}}}} \\{{\xi_{i} = {\sqrt{n_{x}}\left\lbrack {I_{n_{x}}\mspace{14mu} - I_{n_{x}}} \right\rbrack}_{i}}\mspace{104mu}}\end{matrix},} \right.$ wherein in the formula, x is a state variable ofthe GNSS/INS deep coupling filtering model, and a dimension of the x isn_(x), I_(n) _(x) is a n_(x) dimensional unit square matrix,x_(i,k−1|k−1) represents the i^(th) cubature point at k-1 momentspeculated from the measurement at k-1 moment, ζ_(i) is the i^(th)standard cubature point in the CKF filtering, i=1, . . . , 2n_(x),S_(k−1|k−1) satisfies P_(k−1|k−1)=S_(k−1|k−1)(S_(k−1|k−1))^(T).
 6. Themethod according to claim 5, wherein the high-precision nonlinearfiltering method comprises: setting a measurement equation of aniterated nonlinear filtering as:x _(k|k) ^(j+1) =x _(k|k) ^(j)+α_(j) {K _(j)(z _(k) −h(x _(k|k) ^(j))−(p_(k|k−1) ⁻¹ p _(xz,k|k−1))^(T)ϑ_(j))+ϑ_(j)}, wherein x_(k|k) ^(j) , is astate estimate value of the j^(th) measurement iteration at k moment,z_(k) is a measured value output by a phase discriminator of amulti-satellite channel at k moment, h(·) is a measurement equation of adeep coupling tracking loop, ϑ_(j)=x_(k|k) ^(j)−{circumflex over(x)}_(k|k−1) is an error between a posterior estimate and a prioriestimate in the current iteration, K_(j) is a Kalman gain of the jt^(h)iteration, α_(j) is an iterated step-size control coefficient set byaccelerating convergence, satisfying 0α_(j)<1, P_(k|k−1) ⁻¹ is aninverse of P_(k|k−1), P_(xz,k|k−1) is an cross-covariance generatedduring a prediction process, a covariance matrix of state posteriorestimation errors takes a result of the last iteration calculation.